import os
import sys

sys.path.append(".")

import math
import numpy as np
# import converter
from matplotlib import pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import time_processor as tp

Gyro_LSB = 2.88991928100586e-05
Accel_LSB = 7.48188018798828e-06
deg2rad = 3.1415926 / 180.0

def convert(data, type="Gyro"):
    if type == "Gyro":
        return data * Gyro_LSB
    elif type == "Accel":
        return data * Accel_LSB

def read_imu_data(all_data):

    result = []
    all_items = all_data.split(',')
    # print(all_items)

    result.append(convert(float(all_items[5]), "Accel")) 
    result.append(convert(-float(all_items[4]), "Accel")) 
    result.append(convert(float(all_items[3]), "Accel")) 
    iii = all_items[8].split('*')
    result.append(convert(float(iii[0]), "Gyro") * deg2rad) 
    result.append(convert(float(all_items[7]), "Gyro") * deg2rad) 
    result.append(convert(-float(all_items[6]), "Gyro") * deg2rad) 
    

    # print(result)

    return result

def read_gps_time(all_data):
    result = []
    all_items = all_data.split(',')

    result.append(float(all_items[5])) 
    result.append(float(all_items[6])) 

    return result

def read_ins_data2(all_data):

    result = []
    all_items = all_data.split(',')

    result.append(float(all_items[2])) # lat
    result.append(float(all_items[3])) # lon
    result.append(float(all_items[4])) # hei
    result.append(float(all_items[9])) # Roll
    result.append(float(all_items[10])) # Pitch
    result.append(float(all_items[11])) # Yaw

    return result


def read_gnss_data(all_data):

    result = []
    all_items = all_data.split(',')

    result.append(float(all_items[2])) # lat
    result.append(float(all_items[3])) # lon
    result.append(float(all_items[4])) # hei

    return result


def read_data(data_path):
    imu_data = []
    gps_data = []
    ins_data = []
    ins_data2 = []
    num = 0
    with open(data_path) as f:
        for line in f:
            if line[0] != '#':
                pass
            num = num + 1 
            items = line.split(';')
            header_log = items[0].split(',')
            # print(header_log[0])
            # print(num)
            
            if (header_log[0])[1:] == "INSPVAXA": # read the ins data
                t = read_gps_time(items[0])
                d = read_ins_data2(items[1])
                ins_data2.append([t,d])

            if (header_log[0])[1:] == "RAWIMUA":
                t = read_gps_time(items[0])
                d = read_imu_data(items[1])
                imu_data.append([t,d])

            if (header_log[0])[1:] == "BESTPOSA":
                t = read_gps_time(items[0])
                d = read_gnss_data(items[1])
                gps_data.append([t,d])

    return [ins_data2, imu_data, gps_data]
    
    
def save_data(all_data, save_path):

    # ins result
    input_ins = open(save_path+"/ins.txt", "w+")
    for data in all_data[0]:
        input_ins.write("{:.0f}".format(tp.cal2timestamp(tp.gps2cal([(data[0])[0], (data[0])[1]]))))
        input_ins.write(" ")
        input_ins.write("{:.12f}".format((data[1])[0]))
        input_ins.write(" ")
        input_ins.write("{:.12f}".format((data[1])[1]))
        input_ins.write(" ")
        input_ins.write("{:.12f}".format((data[1])[2]))
        input_ins.write(" ")
        input_ins.write("{:.12f}".format((data[1])[3]))
        input_ins.write(" ")
        input_ins.write("{:.12f}".format((data[1])[4]))
        input_ins.write(" ")
        input_ins.write("{:.12f}".format((data[1])[5]))
        input_ins.write("\n")
    input_ins.close()
    
    # imu result
    input_imu = open(save_path+"/imu.txt", "w+")
    input_imu.write("#timestamp [ns],w_RS_S_x [rad s^-1],w_RS_S_y [rad s^-1],w_RS_S_z [rad s^-1],a_RS_S_x [m s^-2],a_RS_S_y [m s^-2],a_RS_S_z [m s^-2]")
    for data in all_data[1]:
        input_imu.write("{:.0f}".format(tp.cal2timestamp(tp.gps2cal([(data[0])[0], (data[0])[1]]))))
        input_imu.write(" ")
        input_imu.write("{:.12f}".format((data[1])[3]))
        input_imu.write(" ")
        input_imu.write("{:.12f}".format((data[1])[4]))
        input_imu.write(" ")
        input_imu.write("{:.12f}".format((data[1])[5]))
        input_imu.write(" ")
        input_imu.write("{:.12f}".format((data[1])[0]))
        input_imu.write(" ")
        input_imu.write("{:.12f}".format((data[1])[1]))
        input_imu.write(" ")
        input_imu.write("{:.12f}".format((data[1])[2]))
        input_imu.write("\n")
    input_imu.close()

    # gps result
    input_gps = open(save_path+"/gps.txt", "w+")
    for data in all_data[2]:
        input_gps.write("{:.0f}".format(tp.cal2timestamp(tp.gps2cal([(data[0])[0], (data[0])[1]]))))
        input_gps.write(" ")
        input_gps.write("{:.12f}".format((data[1])[0]))
        input_gps.write(" ")
        input_gps.write("{:.12f}".format((data[1])[1]))
        input_gps.write(" ")
        input_gps.write("{:.12f}".format((data[1])[2]))
        input_gps.write("\n")
    input_gps.close()

if __name__ == "__main__":
    path = r"/shared/20210902/data/data/"
    read_path = path + r"CONN1-COM5-115200_data.dat"
    save_data(read_data(read_path), path)
